
#include "weightsScheme.h"

//~ namespace MLS {
   
inline Number omega(Number d) {
   
   //~ if (d==NINFINITY) return 0.00000001;
   //~ if (d==0)         return 1e8;
   //~ else 
      //~ return 1.0/(pow(d,2) + 0.00000001);
   
   if (d==NINFINITY) { cout<<"d==NINFINITY"<<endl; return 0.00000001; }
   if (d==0)         { cout<<"d=0"<<endl; return 1e8; }
   else              { //cout<<"d!=0"<<endl; 
      return 1.0/(pow(d,2) + 0.00000001); }
}

Number* compute_WeightsTable(Mesh *model, const Constraints *constraints) {
   
   cout<<" compute_WeightsTable 111"<<endl;
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   for (unsigned int j=0; j<constraints->size(); ++j) {
      model->floodfill_Dijkstra(constraints->ids[j]);
      Vertex_iterator vit = model->vertices_begin();
      for (unsigned int i=0; i<model->nvertices(); ++i) {
         weightsTable[j*texSizetexSize + i] = omega(vit[i].d);
      }
   }
   
   return weightsTable;
}

Number* compute_WeightsTable_Trihedrons(Mesh *model, const Constraints *constraints) {
   
   cout<<" compute_WeightsTable_Trihedrons 222"<<endl;
   
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   const Point3 &a = constraints->positions[0];
   const Point3 &b = constraints->positions[1];
   const Vector3 v(a,b);
   const Number d = sqrt(v*v);
   
   for (unsigned int j=0; j<constraints->size(); j+=4) {
      model->floodfill_Dijkstra(constraints->ids[j]);
      Vertex_iterator vit = model->vertices_begin();
      for (unsigned int i=0; i<model->nvertices(); ++i) {
         Number pd = vit[i].d;
         weightsTable[j*texSizetexSize + i] = omega(pd);
         weightsTable[(j+1)*texSizetexSize + i] = 
         weightsTable[(j+2)*texSizetexSize + i] =
         weightsTable[(j+3)*texSizetexSize + i] = omega(pd+d);
      }
   }
   
   return weightsTable;
}

Number* compute_WeightsTable(MeshWithSkeleton *model, const Constraints *constraints) {
   
   cout<<" compute_WeightsTable 333"<<endl;
   uint texSize = (uint)(std::ceil(std::sqrt(model->skeleton->nJoints())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   for (unsigned int j=0; j<constraints->size(); ++j) {
      model->skeleton->floodfill_Dijkstra(constraints->ids[j]);
      for (unsigned int i=0; i<model->skeleton->nJoints(); ++i) {
         Number pd = model->skeleton->d[i];
         weightsTable[j*texSizetexSize + i] = omega(pd);
      }
   }
   cout<<" compute_WeightsTable(ShapeWithSkeleton *model) ... done!"<<endl;
   return weightsTable;
}

Number* compute_DistancesTable(MeshWithSkeleton *model, const Constraints *constraints) {
   
   cout<<" compute_DistanceTable 444"<<endl;
   uint texSize = (uint)(std::ceil(std::sqrt(model->skeleton->nJoints())));
   uint texSizetexSize = texSize*texSize;
   Number *distancesTable = new Number[texSizetexSize*constraints->size()];
   
   for (unsigned int j=0; j<constraints->size(); ++j) {
      model->skeleton->floodfill_Dijkstra(constraints->ids[j]);
      for (unsigned int i=0; i<model->skeleton->nJoints(); ++i) {
         Number pd = model->skeleton->d[i];
         distancesTable[j*texSizetexSize + i] = pd;
      }
   }
   cout<<" compute_WeightsTable(ShapeWithSkeleton *model) ... done!"<<endl;
   return distancesTable;
}

Number* compute_WeightsTable_Trihedrons(MeshWithSkeleton *model, const Constraints *constraints) {
   
   cout<<" compute_WeightsTable_Trihedrons 555"<<endl;
   
   uint texSize = (uint)(std::ceil(std::sqrt(model->skeleton->nJoints())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   const Point3 &a = constraints->positions[0];
   const Point3 &b = constraints->positions[1];
   const Vector3 v(a,b);
   const Number d = sqrt(v*v);
   
   for (unsigned int j=0; j<constraints->size(); j+=4) {
      model->skeleton->floodfill_Dijkstra(constraints->ids[j]);
      for (unsigned int i=0; i<model->skeleton->nJoints(); ++i) {
         Number pd = model->skeleton->d[i];
         weightsTable[j*texSizetexSize + i] = omega(pd);
         weightsTable[(j+1)*texSizetexSize + i] = 
         weightsTable[(j+2)*texSizetexSize + i] =
         weightsTable[(j+3)*texSizetexSize + i] = omega(pd+d);
      }
   }
   
   return weightsTable;
}

Number* compute_WeightsTable(Mesh *model, Skeleton *skeleton, const Constraints *constraints) {
   
   cout<<" compute_WeightsTable 666"<<endl;
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   //~ Number *weightsTable = new Number[texSizetexSize*skeleton->nJoints()];
   Number *weightsTable = new Number[texSizetexSize*constraints->size()];
   
   Vector3 v;
   for (unsigned int j=0; j<constraints->size(); ++j) {
      //~ const Point3 &P = skeleton->joints[j];
      skeleton->floodfill_Dijkstra(constraints->ids[j]);
      Vertex_iterator vit = model->vertices_begin();
      for (unsigned int i=0; i<model->nvertices(); ++i, ++vit) {
         uint icj = vit->rigging_group; /// the closest joint
         const Point3 &cj = skeleton->joints[icj];
         const Point3 &p = model->getVertexPosition(i);
         v = Vector3(p[0]-cj[0], p[1]-cj[1], p[2]-cj[2]);
         weightsTable[j*texSizetexSize + i] = omega(skeleton->d[icj] + sqrt(v*v));
      }
   }
   
   cout<<" compute_WeightsTable(ShapeWithSkeleton *model) ... done!"<<endl;
   return weightsTable;
}

Number* compute_WeightsTable2(MeshWithSkeleton *model, const Constraints *constraints) {
   
   cout<<" compute_WeightsTable2 newwwwwwwwwwwwwwwwwww 777"<<endl;
   uint texSize = (uint)(std::ceil(std::sqrt(model->nvertices())));
   uint texSizetexSize = texSize*texSize;
   Number *weightsTable = new Number[texSizetexSize*model->skeleton->nBones()];
   
   Skeleton skel(*model->skeleton);
   Vector3 v;
   Vertex_iterator vit = model->vertices_begin();
   for (unsigned int i=0; i<model->nvertices(); ++i, ++vit) {
      //~ cout<<"i: "<<i<<endl;
      // build the associated skeleton modifying its joints appropriately
      for (unsigned int j=1; j<skel.nBones(); ++j) {
         const Bone &b = skel.bones[j];
         const Number &t = vit->ts[j];
         skel.joints[b.first] = model->skeleton->getEllipPointOnBone(j, t);
      }
      // each vertex has its own skeleton
      int idc = vit->rigging_group; // the closest bone 
      //~ cout<<" rigging_group: "<<idc<<endl;
      skel.floodfill_Dijkstra(idc);
      const Point3 &cj = skel.joints[idc];
      const Point3 &p = model->getVertexPosition(i);
      //~ v[0]=p[0]-cj[0]; v[1]=p[1]-cj[1]; v[2]=p[2]-cj[2];
      v = Vector3(p[0]-cj[0], p[1]-cj[1], p[2]-cj[2]);
      for (unsigned int j=0; j<skel.nBones(); ++j) {
         weightsTable[j*texSizetexSize + i] = omega(skel.d[j] + sqrt(v*v));
      }
   }
   
   cout<<" compute_WeightsTable(ShapeWithSkeleton *model) ... done!"<<endl;
   return weightsTable;
}

//~ } // end MLS namespace
